#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import time
import argparse
from threading import Thread, Lock # 导入 Lock 用于线程安全

class VideoStreamPublisher(Node):
    def __init__(self, video_path, topic_name="/camera/color/image_raw", fps=30):
        super().__init__('video_stream_publisher')
        
        # 初始化参数
        self.video_path = video_path
        self.topic_name = topic_name
        self.fps = fps
        self.bridge = CvBridge()
        
        # 创建发布者
        self.publisher_ = self.create_publisher(
            Image, 
            self.topic_name, 
            10
        )
        
        # 视频读取相关
        self.cap = None
        self.is_running = False
        self.reader_thread = None
        self.frame_lock = Lock() # 添加锁，确保帧读取和处理的线程安全
        
        # 日志信息
        self.get_logger().info(f"视频流发布节点已初始化，将从 '{self.video_path}' 读取视频并发布到 '{self.topic_name}'")
        
    def start_streaming(self):
        """启动视频流发布"""
        if self.is_running:
            self.get_logger().warning("视频流已在运行中")
            return
            
        self.get_logger().info(f"尝试打开视频文件: '{self.video_path}'")
        # 打开视频文件
        # 可以尝试使用 GStreamer 管道字符串显式指定后端，但通常不建议，除非你对 GStreamer 很熟悉
        # 例如：self.cap = cv2.VideoCapture(f"filesrc location={self.video_path} ! decodebin ! videoconvert ! appsink", cv2.CAP_GSTREAMER)
        self.cap = cv2.VideoCapture(self.video_path)

        if not self.cap.isOpened():
            self.get_logger().error(f"FATAL ERROR: 无法打开视频文件: '{self.video_path}'")
            # 可以在这里尝试打印更多关于 OpenCV 后端的信息
            self.get_logger().error(f"OpenCV 后端信息: {cv2.getBuildInformation()}")
            return False # 返回 False 表示启动失败
            
        # 获取视频属性
        width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        frame_count = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT))
        video_fps = self.cap.get(cv2.CAP_PROP_FPS) # 获取视频原始帧率
        self.get_logger().info(f"成功打开视频. 视频信息 - 分辨率: {width}x{height}, 总帧数: {frame_count}, 原始帧率: {video_fps}, 目标发布帧率: {self.fps}")
        
        # 启动读取线程
        self.is_running = True
        self.reader_thread = Thread(target=self._read_video_loop)
        self.reader_thread.daemon = True # 设置为守护线程，主程序退出时自动终止
        self.reader_thread.start()
        self.get_logger().info("视频流发布已启动")
        return True # 返回 True 表示启动成功
        
    def stop_streaming(self):
        """停止视频流发布"""
        if not self.is_running:
            return
            
        self.get_logger().info("请求停止视频流发布...")
        self.is_running = False
        
        if self.reader_thread and self.reader_thread.is_alive():
            self.reader_thread.join(timeout=5.0) # 等待线程结束，设置超时
            if self.reader_thread.is_alive():
                self.get_logger().warning("视频读取线程未能正常停止.")
        
        if self.cap and self.cap.isOpened():
            self.cap.release()
            self.get_logger().info("视频文件已释放.")
        self.get_logger().info("视频流发布已停止.")
        
    def _read_video_loop(self):
        """视频读取和发布循环"""
        frame_interval = 1.0 / self.fps  # 每帧间隔时间(秒)
        
        while self.is_running:
            start_time = time.time()
            
            # 使用锁确保对 self.cap 的访问是线程安全的
            with self.frame_lock:
                ret, frame = self.cap.read()

            if not ret:
                self.get_logger().info("视频播放完毕或读取失败，重新从头开始...")
                with self.frame_lock:
                    self.cap.set(cv2.CAP_PROP_POS_FRAMES, 0) # 尝试重置到视频开头
                # 稍微等待一下，避免紧密循环
                time.sleep(0.1) 
                continue
                
            # 转换并发布视频帧
            try:
                ros_image = self.bridge.cv2_to_imgmsg(frame, "bgr8")
                ros_image.header.stamp = self.get_clock().now().to_msg()
                self.publisher_.publish(ros_image)
                # self.get_logger().debug(f"已发布一帧图像 (时间戳: {ros_image.header.stamp.sec}.{ros_image.header.stamp.nanosec})")
            except Exception as e:
                self.get_logger().error(f"发布视频帧时出错: {e}")
                
            # 控制帧率
            elapsed_time = time.time() - start_time
            sleep_time = frame_interval - elapsed_time
            if sleep_time > 0:
                time.sleep(sleep_time)
            # else:
            #     self.get_logger().warn(f"帧处理时间超过目标间隔：{elapsed_time:.4f}s vs {frame_interval:.4f}s")
                
        self.get_logger().info("视频读取循环已退出.")
            
    def __del__(self):
        """析构函数，确保资源释放"""
        self.stop_streaming()

def main(args=None):
    parser = argparse.ArgumentParser(description='ROS2视频流发布节点')
    parser.add_argument('video_path', type=str, help='视频文件路径')
    parser.add_argument('--topic', '-t', default='/camera/color/image_raw', help='发布主题名称')
    parser.add_argument('--fps', '-f', type=int, default=30, help='目标帧率')
    
    # 使用 parse_known_args() 避免解析ROS2特定参数
    ros_args, unknown_args = parser.parse_known_args(args=args) # 捕获未知参数
    
    # 初始化ROS2（传递None或原始参数列表，而非解析后的对象）
    # 将 unknown_args 传递给 rclpy.init，以便 ROS 2 可以处理其自己的命令行参数
    rclpy.init(args=unknown_args) 
    
    video_publisher = None # 预先声明，确保 finally 块可以访问
    try:
        # 创建并启动节点（使用解析后的参数）
        video_publisher = VideoStreamPublisher(
            video_path=ros_args.video_path,
            topic_name=ros_args.topic,
            fps=ros_args.fps
        )
        
        # 只有当启动成功时才进入spin循环
        if video_publisher.start_streaming():
            rclpy.spin(video_publisher)
        else:
            video_publisher.get_logger().error("节点启动失败，将立即退出。")
            # 如果启动失败，直接跳过spin，进入finally块进行清理
            # 确保在失败时也调用 shutdown
            rclpy.shutdown() 
            return # 立即退出 main 函数

    except KeyboardInterrupt:
        video_publisher.get_logger().info("检测到 Ctrl+C, 正在关闭节点...")
    except Exception as e:
        if video_publisher:
            video_publisher.get_logger().error(f"发生未预期错误: {e}")
        else:
            print(f"发生未预期错误 (节点未完全初始化): {e}")
    finally:
        # 清理资源
        if video_publisher:
            video_publisher.stop_streaming()
            video_publisher.destroy_node()
        if rclpy.ok(): # 只有当rclpy还处于活动状态时才尝试shutdown
             rclpy.shutdown()
        print("节点已关闭")

if __name__ == '__main__':
    main()